#ifndef PLINTH_BULLET_H
#define PLINTH_BULLET_H

#include "../core/AbstractObject.h"
#include "../core/RefTable.h"
#include "../core/LuaUtils.h"
#include <list>
#include "btBulletDynamicsCommon.h"
#include "btBulletCollisionCommon.h"

class PlBullet;
class PlConstraint;
class PlShape;

template <class T>
class GeneroStub
{
public:
	int refct;
	T* target;
};

template <class T>
class GeneroRef
{
public:
	GeneroStub<T>* stub;
	GeneroRef(T* src)
	{
		stub = src->getStub();
		++(stub->refct);
	}
	GeneroRef(GeneroRef& rhs)
	{
		stub = rhs.stub;
		++(stub->refct);
	}
	~GeneroRef()
	{
		--(stub->refct);
		if(stub->refct <= 0)
			delete stub;
	}
	T* operator*()
	{
		if(stub)
			return stub->target;
		else
			return NULL;
	}

	T* operator->()
	{
		if(stub)
			return stub->target;
		else
			return NULL;
	}
	void deleteSelf()
	{
		delete this;
	}
};

template <class T>
T* pop_custom_reffed(lua_State *L, int p, const char* name)
{
	GeneroRef<T>* ref = pop_custom<GeneroRef<T>>(L, p, name);
	if(ref == NULL || *(*ref) == NULL)
		return NULL;
	else
		return *(*ref);
}

template <class T>
void push_custom_reffed(lua_State* L, T* val, const char* name)
{
	GeneroRef<T>* ref = new GeneroRef<T>(val);
	push_custom_nocopy<GeneroRef<T>>(L, ref, name);
}

class PlRigidBody
{
public:
	PlRigidBody(PlBullet* world, int slotId);
	~PlRigidBody();

	int getSlotId();
	GeneroStub<PlRigidBody>* getStub();

	btRigidBody* getBtBody();
	
	void addConstraint(PlConstraint* constr);
	void removeConstraint(PlConstraint* constr);
	void setShape(PlShape* shp);

	/* other stuff */
	void addToWorld();
	void removeFromWorld();

    void setMass(double mass);
    void setInertia(btVector3& inertia);

    void setPosition(btVector3& pos);
    void setRotation(btQuaternion& rot);
    
    void setLinearVelocity(const btVector3& vel);
    void setAngularVelocity(const btVector3& rot);

    btVector3& getPosition();
    btQuaternion& getRotation();

	btVector3 getLinearVelocity();
	btVector3 getAngularVelocity();

    void applyGlobalForce(const btVector3& pos, const btVector3& force);
    void applyLocalForce(const btVector3& pos, const btVector3& force);

    void applyGlobalTorque(const btVector3& tq);
    void applyLocalTorque(const btVector3& tq);

    btVector3 localPosToWorld(const btVector3& pos);
    btVector3 worldPosToLocal(const btVector3& pos);
    btVector3 localVecToWorld(const btVector3& vec);
    btVector3 worldVecToLocal(const btVector3& vec);

    void autoCalcInertia();
    void finalizeBody();

private:
	btRigidBody* _theBody;
	btDefaultMotionState* _ms;
	GeneroStub<PlRigidBody>* _stub;
	btTransform _tempTrans;
	btVector3 _pos;
	btQuaternion _rot;
	int _slotid;
	PlBullet* _world;
	PlShape* _shape;
	float _mass;
	btVector3 _inertia;
	bool _inWorld;
	//PlConstraint* _constraints[PL_MAX_CONSTRAINTS_PER_BODY];
	std::list<PlConstraint*> _constraints;

public:
	PlBullet* getWorld(){ return _world;}
	bool inWorld(){ return _inWorld;}
};

class PlConstraint
{
public:
	PlConstraint(PlBullet* world, int slotId);
	~PlConstraint();
	btTypedConstraint* getBtConstraint();
	int getSlotId();
	GeneroStub<PlConstraint>* getStub();
	void detachAllBodies();
	void detachBody(PlRigidBody* body);

	void addToWorld();
	void removeFromWorld();

	void setVector(int slot, btVector3& vec);
	void setQuaternion(int slot, btQuaternion& quat);
	void addBody(int slot, PlRigidBody* body);
	void allowCollisions(bool state);

	void setMotor(int idx, float targetVel, float maxForce);
	void disableMotor(int idx);
	void setLimits(int idx, float low, float high);
	int numMotors();

	void createHinge();
	void createP2P();
	void createSlider();
	void createConeTwist(); // unimplemented at the moment

private:
    const static int CON_NONE = 0;
    const static int CON_P2P = 1;
    const static int CON_HINGE = 2;
    const static int CON_LINEAR = 3;
    const static int CON_DEAD = 10;
	const static int PL_MAX_CONSTRAINT_VECS = 4;
	const static int PL_MAX_CONSTRAINT_QUATS = 4;
	const static int PL_MAX_CONSTRAINT_BODIES = 2;

	int _slotid;
	GeneroStub<PlConstraint>* _stub;
	PlBullet* _world;

	bool _collidable;
	btTypedConstraint* _constraint;
	btVector3 _vecs[PL_MAX_CONSTRAINT_VECS];
	btQuaternion _quats[PL_MAX_CONSTRAINT_QUATS];
	PlRigidBody* _bodies[PL_MAX_CONSTRAINT_BODIES];
	int _conType;
	bool _inWorld;
public:
	PlBullet* getWorld(){ return _world;}
	bool inWorld(){ return _inWorld;}
};

class PlShape
{
public:
	PlShape(PlBullet* world, int slotid, std::string name);
	~PlShape();
	btCollisionShape* getBtShape();
	int getSlotId();
	std::string getName();

	void incrRefCt();
	void decrRefCt();
	int getRefCt();

	void finalize();

    void createBoxShape(double hwx, double hwy, double hwz);
    void createSphereShape(double rad);
    void createCylinderShape(double hwx, double hwy, double hwz);
    void createCapsuleShape(double rad, double h);
    void createConeShape(double rad, double h);
	void createCompoundShape();

	void addSubShape(PlShape* shap, btVector3& pos, btQuaternion& rot, float scale);

private:
	GeneroStub<PlShape>* _stub;
	PlBullet* _world;
	bool _isCompound;
	btCollisionShape* _shape;
	int _slotid;
	std::string _name;
	bool _finalized;
	int _refct;
public:
	GeneroStub<PlShape>* getStub(){ return _stub; }
};

class PlBullet : public AbstractObject
{
public:
	PlBullet();
	virtual ~PlBullet();

	// stuff for creating joints and whatnot
	PlRigidBody* createRigidBody();
	PlShape* createShape(std::string name);
	PlConstraint* createConstraint();

	PlRigidBody* getBody(int id);
	PlShape* getShape(std::string name);
	PlConstraint* getConstraint(int id);

	void deleteBody(PlRigidBody* body);
	void deleteConstraint(PlConstraint* constraint);
	void deleteShape(PlShape* shape);

	void initPhysics();
	void setGravity(btVector3& grav);
	void setInternalTimestep(double dt);
	void stepSimulation(double dt);
	void setMaxSubsteps(int maxSubsteps);
	void setAutoStep(bool autoStep);

	btVector3 makeVector();
	btQuaternion makeQuaternion();
	btTransform makeTransform();

	btDiscreteDynamicsWorld* getWorld();

	// General abstract object stuff goes below

	// Return the c++ type id
	virtual int getCppTypeId();
	// Returns whether this can be cast to a given type
	virtual bool isType(int id);

	// Return the c++ type as a string 
	virtual std::string getCppTypeName();

	// returns a description of this thing
	virtual std::string getDescription();

	// create another one of these
	virtual AbstractObject* newInstance();

	// init & update
	virtual void init();
	virtual void update(double dt);

	// push its static functions into the given lua table
	virtual void pushStaticFuncs(lua_State* L);

private:
	btDiscreteDynamicsWorld* _world;
	RefTable<PlShape> _shapes;
	std::map<std::string, PlShape*> _shapeMap;
	RefTable<PlRigidBody> _bodies;
	RefTable<PlConstraint> _constraints;
	bool _autoUpdating;
	float _timestep;
	int _maxsubsteps;
};

#endif